#include "cartographer/mapping/internal/2d/pose_graph_2d.h"

#include <algorithm>
#include <cmath>
#include <cstdio>
#include <functional>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <limits>
#include <memory>
#include <sstream>
#include <string>

#include "Eigen/Eigenvalues"
#include "absl/memory/memory.h"
#include "cartographer/common/math.h"
#include "cartographer/mapping/internal/2d/overlapping_submaps_trimmer_2d.h"
#include "cartographer/mapping/proto/pose_graph/constraint_builder_options.pb.h"
#include "cartographer/sensor/compressed_point_cloud.h"
#include "cartographer/sensor/internal/voxel_filter.h"
#include "cartographer/transform/transform.h"
#include "glog/logging.h"

namespace cartographer
{
    namespace mapping
    {
        static auto *kWorkQueueDelayMetric = metrics::Gauge::Null();
        static auto *kWorkQueueSizeMetric = metrics::Gauge::Null();
        static auto *kConstraintsSameTrajectoryMetric = metrics::Gauge::Null();
        static auto *kConstraintsDifferentTrajectoryMetric = metrics::Gauge::Null();
        static auto *kActiveSubmapsMetric = metrics::Gauge::Null();
        static auto *kFrozenSubmapsMetric = metrics::Gauge::Null();
        static auto *kDeletedSubmapsMetric = metrics::Gauge::Null();

        /*
        节点类型(两种)：
            1）一帧scan与子图匹配后的位姿(trackingframe)，一个子图内有多个这种scan节点。
            2）子图原点的位姿，即子图节点。
        约束类型(两种)：
            1）scan节点与所在子图原点间的相对变换，即子图内(intra)约束，就是子图内的scan相对于子图的坐标变换。
            2）scan节点与外部子图间的相对变换，即闭环约束(inter)，
        */

        /*
        主逻辑：传入新的节点和各传感器数据后，均新增WorkItem，WorkItem通过回调函数定义一个约束计算任务;任务有依赖关系，在线程
            调度的时候，依赖任务先执行;约束就是点云位姿与子图位姿的相对变换关系;所有约束构建完成后会进行全局优化, 这个全局优化
            与前段ScanMatch一样也是调用ceres计算最小误差;回环检测就是暴力搜索，拿点云和所有子图进行匹配，就是计算点云在子图的
            栅格空间的概率总和，达到一定的阈值认为这个点云与子图存在回环;在回环的暴力搜索过程中使用分支定界法加速搜索;全局优化完
            后会把子图和和点云的全局位姿保存起来;
        概括：
            优化问题可以描述成一个图，图中节点是点云和子图在global系下的位姿，边是点云和子图的相对位姿(即约束)，
            这个约束可是子图内的，也可是回环检测约束。
        */
        PoseGraph2D::PoseGraph2D(const proto::PoseGraphOptions &options,
                                 std::unique_ptr<optimization::OptimizationProblem2D> optimization_problem, common::ThreadPool *thread_pool)
            : options_(options)
            , optimization_problem_(std::move(optimization_problem))
            , constraint_builder_(options_.constraint_builder_options(), thread_pool)
            , thread_pool_(thread_pool)
        {
            if (options.has_overlapping_submaps_trimmer_2d())
            {
                const auto &trimmer_options = options.overlapping_submaps_trimmer_2d();
                AddTrimmer(absl::make_unique<OverlappingSubmapsTrimmer2D>(
                    trimmer_options.fresh_submaps_count(), trimmer_options.min_covered_area(), trimmer_options.min_added_submaps_count()));
            }
        }

        PoseGraph2D::~PoseGraph2D()
        {
            WaitForAllComputations();
            absl::MutexLock locker(&work_queue_mutex_);
            CHECK(work_queue_ == nullptr);
        }

        std::vector<SubmapId> PoseGraph2D::InitializeGlobalSubmapPoses(const int trajectory_id, const common::Time time,
                                                                       const Submap2dSet &insertion_submaps)
        {
            CHECK(!insertion_submaps.empty());
            const auto &submap_data = optimization_problem_->submap_data();
            if (insertion_submaps.size() == 1)
            {
                // case1 轨迹初始时，前端只有一个子图
                if (submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0)
                {
                    // 如该轨迹有设置初始的位姿(这个初始的位姿是相对于其他轨迹的)
                    if (data_.initial_trajectory_poses.count(trajectory_id) > 0)
                    {
                        auto other_traj_id = data_.initial_trajectory_poses.at(trajectory_id).to_trajectory_id;
                        // 把这两个轨迹建立一个连接
                        data_.trajectory_connectivity_state.Connect(trajectory_id, other_traj_id, time);
                        LOG(INFO) << "initPose connect_traj " << trajectory_id << " " << other_traj_id;
                    }
                    // 把子图的全局位姿加入到优化问题；子图和节点的全局位姿计算逻辑相同
                    const auto &tf_local_global = ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, trajectory_id);
                    const auto &curr_local_pose = insertion_submaps[0]->local_pose();
                    auto &tfvec = tf_local_global.translation();
                    auto &locvec = curr_local_pose.translation();
                    LOG(INFO) << "[pg2_init] global_submap size " << data_.global_submap_poses_2d.size();
                    LOG(INFO) << "[pg2_init] local2global " << tfvec(0) << " " << tfvec(1) << " " << tfvec(2);
                    LOG(INFO) << "[pg2_init] localpose " << locvec(0) << " " << locvec(1) << " " << locvec(2);
                    optimization_problem_->AddSubmap(trajectory_id, transform::Project2D(tf_local_global * curr_local_pose));
                }
                CHECK_EQ(1, submap_data.SizeOfTrajectoryOrZero(trajectory_id));
                const SubmapId submap_id{trajectory_id, 0};
                CHECK(data_.submap_data.at(submap_id).submap == insertion_submaps.front());
                return {submap_id};
            }
            // 确保前端有两个子图，此时第一个子图已完成
            CHECK_EQ(2, insertion_submaps.size());
            const auto end_it = submap_data.EndOfTrajectory(trajectory_id);
            // 确保已有优化后的子图
            CHECK(submap_data.BeginOfTrajectory(trajectory_id) != end_it);
            // EndOfTrajectory是下一个轨迹的起始索引，所以std::prev(end_it)就是当前轨迹最后一个索引
            const SubmapId last_submap_id = std::prev(end_it)->id;
            if (data_.submap_data.at(last_submap_id).submap == insertion_submaps.front())
            {
                // case2 前端有两个子图，但只有第一个子图加入到data_.submap_data中
                const auto &first_submap_pose = submap_data.at(last_submap_id).global_pose;
                const auto &local_submap_pose0 = constraints::ComputeSubmapPose(*insertion_submaps[0]);
                const auto &local_submap_pose1 = constraints::ComputeSubmapPose(*insertion_submaps[1]);
                optimization_problem_->AddSubmap(trajectory_id, first_submap_pose * local_submap_pose0.inverse() * local_submap_pose1);
                return {last_submap_id, SubmapId{trajectory_id, last_submap_id.submap_index + 1}};
            }
            CHECK(data_.submap_data.at(last_submap_id).submap == insertion_submaps.back());
            const SubmapId front_submap_id{trajectory_id, last_submap_id.submap_index - 1};
            CHECK(data_.submap_data.at(front_submap_id).submap == insertion_submaps.front());
            // 返回前端的两个子图
            return {front_submap_id, last_submap_id};
        }

        // 前端成功匹配一帧点云就调用一次这个函数，也即每一个点云位姿节点都会加入到后端图优化中
        // 子图并不是每次都添加，初始时前端仅一个子图，且后端无子图，此时调用该函数会把前端初始的子图加进来；
        // 后端加入子图是保存子图的智能指针(不拷贝子图数据);通过后端的最后一个子图指针与前端最后一个子图指针进行比较，如果不同
        // 则将子图加入到后端，这样可以确保前端新增的所有子图都被添加后端，所以只需要取insertion_submaps.back()判断就行了，因为
        // insertion_submaps.front()在新增后已经被添加进来了
        NodeId PoseGraph2D::AppendNode(std::shared_ptr<const TrajectoryNode::Data> constant_data, const int trajectory_id,
                                       const Submap2dSet &insertion_submaps, const transform::Rigid3d &optimized_pose)
        {
            absl::MutexLock locker(&mutex_);
            // 如果轨迹不存在, 则将轨迹添加到连接状态里并添加采样器。这个轨迹并不是指前端部分的轨迹，在后端
            // 优化部分(PoseGraph)里有专门用来构建约束的"轨迹"，即由若干节点连接的轨迹。
            AddTrajectoryIfNeeded(trajectory_id);
            if (!CanAddWorkItemModifying(trajectory_id))
            {
                LOG(WARNING) << "AddNode was called for finished or deleted trajectory.";
            }
            // 将点云位姿节点加入当前轨迹中
            const NodeId node_id = data_.trajectory_nodes.Append(trajectory_id, TrajectoryNode{constant_data, optimized_pose});
            ++data_.num_trajectory_nodes;
            LOG(INFO) << "[Pg2] AppendNode " << node_id << " localMapSize " << insertion_submaps.size();
            // 刚开始轨迹没有子图或者前端子图的最后一个没有被添加则添加子图;一个子图被新建还是需要比较久的时间的,因为需要点云帧积累到一定的数量;
            if (data_.submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 ||
                std::prev(data_.submap_data.EndOfTrajectory(trajectory_id))->data.submap != insertion_submaps.back())
            {
                // We grow 'data_.submap_data' as needed. This code assumes that the first
                // time we see a new submap is as 'insertion_submaps.back()'.
                const auto submap_id = data_.submap_data.Append(trajectory_id, InternalSubmapData());
                data_.submap_data.at(submap_id).submap = insertion_submaps.back();
                LOG(INFO) << "[Pg2] AppendNode add submap " << submap_id;
                kActiveSubmapsMetric->Increment();
            }
            return node_id;
        }

        /// @brief 在GlobalTrajectoryBuilder中被调用;前端匹配成功后返回匹配结果(包括点云数据，两个子图，local位姿等);
        /// GlobalTrajectoryBuilder将前端匹配结果直接给到该函数
        /// @param constant_data  匹配后的数据，包括时间,点云和local系的pose
        /// @param trajectory_id
        /// @param insertion_submaps
        /// @return
        NodeId PoseGraph2D::AddNode(std::shared_ptr<const TrajectoryNode::Data> constant_data, const int trajectory_id,
                                    const Submap2dSet &insertion_submaps)
        {
            // 局部系是前端匹配所在系，全局系是map所在系，局部系的位姿是前端通过scan-to-map计算的，所以局部系的位姿都是没有经过
            // 优化的；而全局系的位姿是后端基于前端给的位姿进行图优化后的位姿;起始时刻，局部系与全局系坐标是重合的，随着后端优化
            // 进行，全局系与局部系产生的漂移，这个漂移量就是odom到map的变换，也是局部系到全局系的变换量，也是函数
            // GetLocalToGlobalTransform的返回结果，该函数内用最后一个已优化的子图的全局位姿和其局部位姿的差量作为local到global的
            // 变换;已优化的子图才有全局位姿

            // 点云节点的全局位姿
            const transform::Rigid3d optimized_pose(GetLocalToGlobalTransform(trajectory_id) * constant_data->local_pose);
            // 加入节点, 并得到节点的id;这里的"节点"是后端优化模块的概念;
            const NodeId node_id = AppendNode(constant_data, trajectory_id, insertion_submaps, optimized_pose);
            // 构建该点云节点和前端第一子图的intra约束
            // 构建该点云节点和所有已完成子图的inter约束(回环检测)
            // 如果前端第一子图已完成，则该子图会与所有节点计算inter约束
            const bool newly_finished_submap = insertion_submaps.front()->insertion_finished();
            // 添加约束计算任务到队列中等待线程池调度
            AddWorkItem(
                [=]() LOCKS_EXCLUDED(mutex_) { return ComputeConstraintsForNode(node_id, insertion_submaps, newly_finished_submap); });
            return node_id;
        }

        /*
         * 后端优化的操作基本都是采用线程池在后台运行，将任务放入任务队列中，然后排队调用执行
         */
        void PoseGraph2D::AddWorkItem(const std::function<WorkItem::Result()> &work_item)
        {
            absl::MutexLock locker(&work_queue_mutex_);
            if (work_queue_ == nullptr)
            {
                // 如果工作队列未被初始化，则先初始化并启动线程执行任务队列
                work_queue_ = absl::make_unique<WorkQueue>();
                auto task = absl::make_unique<common::Task>();
                task->SetWorkItem([this]() { DrainWorkQueue(); });
                thread_pool_->Schedule(std::move(task));
            }
            // 将当前任务插入到工作队列中，等待执行
            const auto now = std::chrono::steady_clock::now();
            work_queue_->push_back({now, work_item});
            kWorkQueueSizeMetric->Set(work_queue_->size());
            kWorkQueueDelayMetric->Set(std::chrono::duration_cast<std::chrono::duration<double>>(now - work_queue_->front().time).count());
        }

        void PoseGraph2D::AddTrajectoryIfNeeded(const int trajectory_id)
        {
            data_.trajectories_state[trajectory_id];
            CHECK(data_.trajectories_state.at(trajectory_id).state != TrajectoryState::FINISHED);
            CHECK(data_.trajectories_state.at(trajectory_id).state != TrajectoryState::DELETED);
            CHECK(data_.trajectories_state.at(trajectory_id).deletion_state == InternalTrajectoryState::DeletionState::NORMAL);
            data_.trajectory_connectivity_state.Add(trajectory_id);
            // Make sure we have a sampler for this trajectory.
            if (!global_localization_samplers_[trajectory_id])
            {
                global_localization_samplers_[trajectory_id] =
                    absl::make_unique<common::FixedRatioSampler>(options_.global_sampling_ratio());
            }
        }

        void PoseGraph2D::AddImuData(const int trajectory_id, const sensor::ImuData &imu_data)
        {
            AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                if (CanAddWorkItemModifying(trajectory_id))
                {
                    optimization_problem_->AddImuData(trajectory_id, imu_data);
                }
                return WorkItem::Result::kDoNotRunOptimization;
            });
        }

        void PoseGraph2D::AddOdometryData(const int trajectory_id, const sensor::OdometryData &odometry_data)
        {
            AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                if (CanAddWorkItemModifying(trajectory_id))
                {
                    optimization_problem_->AddOdometryData(trajectory_id, odometry_data);
                }
                return WorkItem::Result::kDoNotRunOptimization;
            });
        }

        void PoseGraph2D::AddFixedFramePoseData(const int trajectory_id, const sensor::FixedFramePoseData &fixed_frame_pose_data)
        {
            AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                if (CanAddWorkItemModifying(trajectory_id))
                {
                    optimization_problem_->AddFixedFramePoseData(trajectory_id, fixed_frame_pose_data);
                }
                return WorkItem::Result::kDoNotRunOptimization;
            });
        }

        void PoseGraph2D::AddLandmarkData(int trajectory_id, const sensor::LandmarkData &landmark_data)
        {
            AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                if (CanAddWorkItemModifying(trajectory_id))
                {
                    for (const auto &observation : landmark_data.landmark_observations)
                    {
                        data_.landmark_nodes[observation.id].landmark_observations.emplace_back(
                            PoseGraphInterface::LandmarkNode::LandmarkObservation{
                                trajectory_id, landmark_data.time, observation.landmark_to_tracking_transform,
                                observation.translation_weight, observation.rotation_weight});
                    }
                }
                return WorkItem::Result::kDoNotRunOptimization;
            });
        }

        // 回环检测
        void PoseGraph2D::ComputeConstraint(const NodeId &node_id, const SubmapId &submap_id)
        {
            bool maybe_add_local_constraint = false;
            bool maybe_add_global_constraint = false;
            const TrajectoryNode::Data *constant_data;
            const Submap2D *submap;
            {
                absl::MutexLock locker(&mutex_);
                CHECK(data_.submap_data.at(submap_id).state == SubmapState::kFinished);
                if (!data_.submap_data.at(submap_id).submap->insertion_finished())
                {
                    // 如果是未完成状态的地图不进行约束计算
                    // Uplink server only receives grids when they are finished, so skip
                    // constraint search before that.
                    return;
                }
                // 该node和该submap中的所有node的比较时间，取最新的时间;因为这是个公共函数，所以点云节点时间和子图中节点的最新时间
                // 都可能是旧的时间，所以需要进行比较
                const common::Time node_time = GetLatestNodeTime(node_id, submap_id);
                // 两个轨迹的最后连接时间;在定位模式初始时第一个子图还没初始化完成，新增的点云节点会与先验地图中的子图进行回环检测，一旦回环检测成功
                // 先验地图轨迹与新轨迹便建立连接，而且两条轨迹每次发生回环都会更新连接时间，所以在下面的逻辑中，如果节点和子图属于不同轨迹，但距离
                // 最后连接时间比较近，则认为当前节点仍然有可能与这个子图产生回环;
                const common::Time last_connection_time =
                    data_.trajectory_connectivity_state.LastConnectionTime(node_id.trajectory_id, submap_id.trajectory_id);
                // 如果节点和子图属于同一轨迹, 或者不属于同一轨迹，但是该点云节点距离两个轨迹建立连接的时间很近，则进行局部搜索；
                // 如果两个轨迹成功进行了回环，那么两条轨迹会有子图和点云节点构成连接，如此时当前节点接近这个回环里的子图或者点云
                // 的时间，那么当前节点很可能通过局部搜索也能建立回环；
                if (node_id.trajectory_id == submap_id.trajectory_id ||
                    node_time < last_connection_time + common::FromSeconds(options_.global_constraint_search_after_n_seconds()))
                {
                    // If the node and the submap belong to the same trajectory or if there
                    // has been a recent global constraint that ties that node's trajectory to
                    // the submap's trajectory, it suffices to do a match constrained to a
                    // local search window.
                    maybe_add_local_constraint = true;
                }
                // 如果两个不属于同一条轨迹 并且最近也没有什么连接. 则说明局部窗口搜索很可能连接不上,因此需要进行全局搜索.
                // 控制全局搜索的频率，因为全局搜索是很费时的
                else if (global_localization_samplers_[node_id.trajectory_id]->Pulse())
                {
                    maybe_add_global_constraint = true;
                }
                // 获取节点信息数据与地图数据
                constant_data = data_.trajectory_nodes.at(node_id).constant_data.get();
                submap = static_cast<const Submap2D *>(data_.submap_data.at(submap_id).submap.get());
            }
            if (maybe_add_local_constraint)
            {
                // 建图模式时执行, node_id.trajectory_id与submap_id.trajectory_id是相同的，last_connection_time没有意义会被忽略
                // submap原点在global坐标系下的坐标的逆 * 节点在global坐标系下的坐标 = submap原点与节点的坐标变换;注意回环优化的
                // 位姿是相对位姿，这里用子图和节点的全局位姿计算了一个初始的相对位姿;在调用该函数前，这个节点和子图都已经加入到
                // optimization_problem_中
                const transform::Rigid2d initial_relative_pose = optimization_problem_->submap_data().at(submap_id).global_pose.inverse() *
                                                                 optimization_problem_->node_data().at(node_id).global_pose_2d;
                // 计算回环约束，通过局部搜索进行回环检测(对局部子图进行回环检测)
                constraint_builder_.MaybeAddConstraint(submap_id, submap, node_id, constant_data, initial_relative_pose);
            }
            else if (maybe_add_global_constraint)
            {
                // 定位模式时执行, node_id.trajectory_id与submap_id.trajectory_id是不一致的
                // 全局搜索窗口的约束计算 (对整体子图进行回环检测)
                constraint_builder_.MaybeAddGlobalConstraint(submap_id, submap, node_id, constant_data);
            }
        }

        // 1，计算子图内约束
        // 2，节点与已完成子图进行回环检测
        // 3，子图与已优化节点进行回环检测，定位模式时，如果optimize_every_n_nodes的配置比子图完成阈值(num_range_data)小，则会先进行2的回环，否则先进行3的回环
        // 在定位模式，optimize_every_n_nodes和num_range_data的值都不要太小，因为其大小决定了什么时候进行第一次优化;如果第一次优化延迟太久，那么前端的
        // 累计误差会越来越大
        WorkItem::Result PoseGraph2D::ComputeConstraintsForNode(const NodeId &node_id, Submap2dSet insertion_submaps,
                                                                const bool newly_finished_submap)
        {
            std::vector<SubmapId> submap_ids;
            std::vector<SubmapId> finished_submap_ids;
            std::set<NodeId> newly_finished_submap_node_ids;
            int curr_traj_submap_size = 0;
            {
                absl::MutexLock locker(&mutex_);
                // 得到该节点的前端匹配数据.
                const auto &constant_data = data_.trajectory_nodes.at(node_id).constant_data;
                // 如该子图刚进入后端，则计算子图的全局位姿，然后把子图加入到优化问题,第一个子图的位姿为0
                submap_ids = InitializeGlobalSubmapPoses(node_id.trajectory_id, constant_data->time, insertion_submaps);
                CHECK_EQ(submap_ids.size(), insertion_submaps.size());
                // 前端的第一个子图
                const SubmapId matching_id = submap_ids.front();
                // 当前节点在重力系的位姿
                const transform::Rigid2d local_pose_2d = transform::Project2D(
                    constant_data->local_pose * transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse()));
                // 利用子图的全局位姿计算当前点云节点的全局位姿
                const transform::Rigid2d global_pose_2d = optimization_problem_->submap_data().at(matching_id).global_pose *
                                                          constraints::ComputeSubmapPose(*insertion_submaps.front()).inverse() *
                                                          local_pose_2d;
                // 把当前节点加入到优化问题，子图已经在前面加入到优化问题了；优化问题中的位姿都是经过重力对齐的
                // 注意前端的点云数据不会被加入到优化问题中
                optimization_problem_->AddTrajectoryNode(
                    matching_id.trajectory_id,
                    optimization::NodeSpec2D{constant_data->time, local_pose_2d, global_pose_2d, constant_data->gravity_alignment});
                // 加入当前节点和插入的子图之间的约束.插入的子图一般是两个
                for (size_t i = 0; i < insertion_submaps.size(); ++i)
                {
                    const SubmapId submap_id = submap_ids[i];
                    // Even if this was the last node added to 'submap_id', the submap will
                    // only be marked as finished in 'data_.submap_data' further below.
                    CHECK(data_.submap_data.at(submap_id).state == SubmapState::kNoConstraintSearch);
                    data_.submap_data.at(submap_id).node_ids.emplace(node_id);
                    // 计算点云与子图的相对位姿
                    const transform::Rigid2d constraint_transform =
                        constraints::ComputeSubmapPose(*insertion_submaps[i]).inverse() * local_pose_2d;
                    // 点云与子图的相对位姿作为约束
                    data_.constraints.push_back(Constraint{submap_id,
                                                           node_id,
                                                           {transform::Embed3D(constraint_transform), options_.matcher_translation_weight(),
                                                            options_.matcher_rotation_weight()},
                                                           Constraint::INTRA_SUBMAP});
                }
                // 搜集所有已完成submap的ID
                for (const auto &submap_id_data : data_.submap_data)
                {
                    if (submap_id_data.id.trajectory_id == node_id.trajectory_id)
                        curr_traj_submap_size++;
                    if (submap_id_data.data.state == SubmapState::kFinished)
                    {
                        CHECK_EQ(submap_id_data.data.node_ids.count(node_id), 0);
                        finished_submap_ids.emplace_back(submap_id_data.id);
                    }
                }
                // 如果最新的子图已经完成,把state设置为完成.
                if (newly_finished_submap)
                {
                    const SubmapId newly_finished_submap_id = submap_ids.front();
                    InternalSubmapData &finished_submap_data = data_.submap_data.at(newly_finished_submap_id);
                    CHECK(finished_submap_data.state == SubmapState::kNoConstraintSearch);
                    finished_submap_data.state = SubmapState::kFinished;
                    newly_finished_submap_node_ids = finished_submap_data.node_ids;
                }
            }
            size_t all_submap_size = data_.submap_data.size();
            size_t insert_submap_size = insertion_submaps.size();
            size_t finished_submap_size = finished_submap_ids.size();
            LOG(INFO) << "submap_size: insert " << insert_submap_size << " all " << all_submap_size << " finished " << finished_submap_size
                      << " curr_traj " << curr_traj_submap_size;
            // 点云节点与所有已完成子图进行回环检测;纯定位模式时，之前地图的子图都是finished，函数ComputeConstraint的
            // 调用与点云帧同频;
            for (const auto &submap_id : finished_submap_ids)
            {
                ComputeConstraint(node_id, submap_id);
                // Print("interConst1", node_id, submap_id);
            }
            // 新完成子图与过去所有节点(节点不属于这个新完成的子图)进行回环检测，因为一个子图完成需要一定数量的点云帧积累，所以这个计算频率较低;
            if (newly_finished_submap)
            {
                const SubmapId newly_finished_submap_id = submap_ids.front();
                // We have a new completed submap, so we look into adding constraints for old nodes.
                for (const auto &node_id_data : optimization_problem_->node_data())
                {
                    const NodeId &node_id = node_id_data.id;
                    // 如果这个节点就在这个新完成子图内则跳过
                    if (newly_finished_submap_node_ids.count(node_id) == 0)
                    {
                        ComputeConstraint(node_id, newly_finished_submap_id);
                        Print("[interConst2]", node_id, newly_finished_submap_id);
                    }
                }
            }
            // 让ConstraintBuilder2D内部创建一个计数任务.
            constraint_builder_.NotifyEndOfNode();
            absl::MutexLock locker(&mutex_);
            // 判断是否需要进行优化.
            ++num_nodes_since_last_loop_closure_;
            // 自从上次回环检测后，累积的节点数量超过阈值则需要进行一次全局优化;这里是触发优化的唯一条件，定位模式时optimize_every_n_nodes的值
            // 比建图模式时的值小
            if (options_.optimize_every_n_nodes() > 0 && num_nodes_since_last_loop_closure_ > options_.optimize_every_n_nodes())
            {
                return WorkItem::Result::kRunOptimization;
            }
            return WorkItem::Result::kDoNotRunOptimization;
        }

        common::Time PoseGraph2D::GetLatestNodeTime(const NodeId &node_id, const SubmapId &submap_id) const
        {
            common::Time time = data_.trajectory_nodes.at(node_id).constant_data->time;
            const InternalSubmapData &submap_data = data_.submap_data.at(submap_id);
            if (!submap_data.node_ids.empty())
            {
                const NodeId last_submap_node_id = *data_.submap_data.at(submap_id).node_ids.rbegin();
                time = std::max(time, data_.trajectory_nodes.at(last_submap_node_id).constant_data->time);
            }
            return time;
        }

        void PoseGraph2D::UpdateTrajectoryConnectivity(const Constraint &constraint)
        {
            CHECK_EQ(constraint.tag, Constraint::INTER_SUBMAP);
            const common::Time time = GetLatestNodeTime(constraint.node_id, constraint.submap_id);
            data_.trajectory_connectivity_state.Connect(constraint.node_id.trajectory_id, constraint.submap_id.trajectory_id, time);
        }

        void PoseGraph2D::DeleteTrajectoriesIfNeeded()
        {
            TrimmingHandle trimming_handle(this);
            for (auto &it : data_.trajectories_state)
            {
                if (it.second.deletion_state == InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION)
                {
                    // TODO(gaschler): Consider directly deleting from data_, which may be
                    // more complete.
                    auto submap_ids = trimming_handle.GetSubmapIds(it.first);
                    for (auto &submap_id : submap_ids)
                    {
                        trimming_handle.TrimSubmap(submap_id);
                    }
                    it.second.state = TrajectoryState::DELETED;
                    it.second.deletion_state = InternalTrajectoryState::DeletionState::NORMAL;
                }
            }
        }

        // 该函数与函数DrainWorkQueue连续地互相调用;DrainWorkQueue函数是不断得取出队列里的约束计算任务进行执行,当DrainWorkQueue
        // 里面发现需要全局优化则退出循环，然后调用HandleWorkQueue,函数HandleWorkQueue内进行一次优化，优化结束后再调用DrainWorkQueue
        // 也就意味着优化与约束计算任务不是同步的，而是交替进行的, 这就是该类的主逻辑;
        void PoseGraph2D::HandleWorkQueue(const constraints::ConstraintBuilder2D::Result &result)
        {
            int costSizeIntra = 0;
            int costSizeInter = 0;
            int costSizeDiffTraj = 0;
            for (auto &cost : result)
            {
                if (cost.tag == CostTag::INTRA_SUBMAP)
                    costSizeIntra++;
                if (cost.tag == CostTag::INTER_SUBMAP)
                {
                    costSizeInter++;
                    if (cost.node_id.trajectory_id != 0)
                        costSizeDiffTraj++;
                }
            }
            LOG(INFO) << "AddCostSize " << result.size();
            LOG(INFO) << "new intra " << costSizeIntra << " inter " << costSizeInter << " diffTraj " << costSizeDiffTraj;
            costSizeIntra = 0;
            costSizeInter = 0;
            costSizeDiffTraj = 0;
            for (auto &cost : data_.constraints)
            {
                if (cost.tag == CostTag::INTRA_SUBMAP)
                    costSizeIntra++;
                if (cost.tag == CostTag::INTER_SUBMAP)
                {
                    costSizeInter++;
                    if (cost.node_id.trajectory_id != 0)
                        costSizeDiffTraj++;
                }
            }
            LOG(INFO) << "org intra " << costSizeIntra << " inter " << costSizeInter << " diffTraj " << costSizeDiffTraj;
            {
                absl::MutexLock locker(&mutex_);
                data_.constraints.insert(data_.constraints.end(), result.begin(), result.end());
            }
            RunOptimization(); // 进行全局优化
            if (global_slam_optimization_callback_)
            {
                std::map<int, NodeId> trajectory_id_to_last_optimized_node_id;
                std::map<int, SubmapId> trajectory_id_to_last_optimized_submap_id;
                {
                    absl::MutexLock locker(&mutex_);
                    const auto &submap_data = optimization_problem_->submap_data();
                    const auto &node_data = optimization_problem_->node_data();
                    for (const int trajectory_id : node_data.trajectory_ids())
                    {
                        if (node_data.SizeOfTrajectoryOrZero(trajectory_id) == 0 || submap_data.SizeOfTrajectoryOrZero(trajectory_id) == 0)
                        {
                            continue;
                        }
                        trajectory_id_to_last_optimized_node_id.emplace(trajectory_id,
                                                                        std::prev(node_data.EndOfTrajectory(trajectory_id))->id);
                        trajectory_id_to_last_optimized_submap_id.emplace(trajectory_id,
                                                                          std::prev(submap_data.EndOfTrajectory(trajectory_id))->id);
                    }
                }
                global_slam_optimization_callback_(trajectory_id_to_last_optimized_submap_id, trajectory_id_to_last_optimized_node_id);
            }

            {
                absl::MutexLock locker(&mutex_);
                for (const Constraint &constraint : result)
                {
                    UpdateTrajectoryConnectivity(constraint);
                }
                DeleteTrajectoriesIfNeeded();
                // 定位模式会裁剪子图，只留最后的三个子图
                TrimmingHandle trimming_handle(this);
                for (auto &trimmer : trimmers_)
                {
                    trimmer->Trim(&trimming_handle);
                }
                trimmers_.erase(std::remove_if(trimmers_.begin(), trimmers_.end(),
                                               [](std::unique_ptr<PoseGraphTrimmer> &trimmer) { return trimmer->IsFinished(); }),
                                trimmers_.end());
                num_nodes_since_last_loop_closure_ = 0;

                // Update the gauges that count the current number of constraints.
                double inter_constraints_same_trajectory = 0;
                double inter_constraints_different_trajectory = 0;
                for (const auto &constraint : data_.constraints)
                {
                    if (constraint.tag == cartographer::mapping::PoseGraph::Constraint::INTRA_SUBMAP)
                    {
                        continue;
                    }
                    if (constraint.node_id.trajectory_id == constraint.submap_id.trajectory_id)
                    {
                        ++inter_constraints_same_trajectory;
                    }
                    else
                    {
                        ++inter_constraints_different_trajectory;
                    }
                }
                kConstraintsSameTrajectoryMetric->Set(inter_constraints_same_trajectory);
                kConstraintsDifferentTrajectoryMetric->Set(inter_constraints_different_trajectory);
            }
            DrainWorkQueue();
        }

        // 在加入第一个约束任务到队列时，工作队列未初始化，初始化工作队列后立即创建一个任务进行调度，这个任务就是调用
        // 函数DrainWorkQueue；该函数里面当工作队列为空或者需要进行全局优化才会结束，而全局优化完会再次调用该函数;
        // 所以这个函数是由添加任务和全局优化结束这两个条件进行触发的，在定位和建图的过程中，后端会持续地创建约束任务，
        // 所以正常情况下，都是全局优化和该函数互相调用；也就意味着全局优化和约束计算不是同时进行的;这里在进行全局优化
        // 前会先把constraint_builder_里面计算的回环约束都拷贝出来；
        void PoseGraph2D::DrainWorkQueue()
        {
            bool process_work_queue = true;
            size_t work_queue_size;
            // 任务的内容是计算约束
            while (process_work_queue)
            {
                std::function<WorkItem::Result()> work_item;
                {
                    absl::MutexLock locker(&work_queue_mutex_);
                    if (work_queue_->empty())
                    {
                        work_queue_.reset();
                        return;
                    }
                    work_item = work_queue_->front().task;
                    work_queue_->pop_front();
                    work_queue_size = work_queue_->size();
                    kWorkQueueSizeMetric->Set(work_queue_size);
                }
                // 如需要执行优化则退出循环取执行优化，一般是由于加入的节点累积超过阈值则需要进行一次优化
                process_work_queue = work_item() == WorkItem::Result::kDoNotRunOptimization;
            }
            LOG(INFO) << "Remaining work items in queue: " << work_queue_size;
            // 执行到这里说明队列里面的任务在执行后触发优化，但是这里不会立马执行函数HandleWorkQueue，下面的函数其实还是往队列里面加一个whenDone任务；
            // 该任务就是把计算的约束都拷贝出来
            constraint_builder_.WhenDone([this](const constraints::ConstraintBuilder2D::Result &result) { HandleWorkQueue(result); });
        }

        void PoseGraph2D::WaitForAllComputations()
        {
            int num_trajectory_nodes;
            {
                absl::MutexLock locker(&mutex_);
                num_trajectory_nodes = data_.num_trajectory_nodes;
            }
            const int num_finished_nodes_at_start = constraint_builder_.GetNumFinishedNodes();
            auto report_progress = [this, num_trajectory_nodes, num_finished_nodes_at_start]() {
                // Log progress on nodes only when we are actually processing nodes.
                if (num_trajectory_nodes != num_finished_nodes_at_start)
                {
                    std::ostringstream progress_info;
                    progress_info << "Optimizing: " << std::fixed << std::setprecision(1)
                                  << 100. * (constraint_builder_.GetNumFinishedNodes() - num_finished_nodes_at_start) /
                                         (num_trajectory_nodes - num_finished_nodes_at_start)
                                  << "%...";
                    std::cout << "\r\x1b[K" << progress_info.str() << std::flush;
                }
            };

            // First wait for the work queue to drain so that it's safe to schedule
            // a WhenDone() callback.
            {
                const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(work_queue_mutex_) { return work_queue_ == nullptr; };
                absl::MutexLock locker(&work_queue_mutex_);
                while (!work_queue_mutex_.AwaitWithTimeout(absl::Condition(&predicate), absl::FromChrono(common::FromSeconds(1.))))
                {
                    report_progress();
                }
            }

            // Now wait for any pending constraint computations to finish.
            absl::MutexLock locker(&mutex_);
            bool notification = false;
            constraint_builder_.WhenDone([this, &notification](const constraints::ConstraintBuilder2D::Result &result)
                                             LOCKS_EXCLUDED(mutex_) {
                                                 absl::MutexLock locker(&mutex_);
                                                 data_.constraints.insert(data_.constraints.end(), result.begin(), result.end());
                                                 notification = true;
                                             });
            const auto predicate = [&notification]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) { return notification; };
            while (!mutex_.AwaitWithTimeout(absl::Condition(&predicate), absl::FromChrono(common::FromSeconds(1.))))
            {
                report_progress();
            }
            CHECK_EQ(constraint_builder_.GetNumFinishedNodes(), num_trajectory_nodes);
            std::cout << "\r\x1b[KOptimizing: Done.     " << std::endl;
        }

        void PoseGraph2D::DeleteTrajectory(const int trajectory_id)
        {
            {
                absl::MutexLock locker(&mutex_);
                auto it = data_.trajectories_state.find(trajectory_id);
                if (it == data_.trajectories_state.end())
                {
                    LOG(WARNING) << "Skipping request to delete non-existing trajectory_id: " << trajectory_id;
                    return;
                }
                it->second.deletion_state = InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION;
            }
            AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                CHECK(data_.trajectories_state.at(trajectory_id).state != TrajectoryState::ACTIVE);
                CHECK(data_.trajectories_state.at(trajectory_id).state != TrajectoryState::DELETED);
                CHECK(data_.trajectories_state.at(trajectory_id).deletion_state ==
                      InternalTrajectoryState::DeletionState::SCHEDULED_FOR_DELETION);
                data_.trajectories_state.at(trajectory_id).deletion_state = InternalTrajectoryState::DeletionState::WAIT_FOR_DELETION;
                return WorkItem::Result::kDoNotRunOptimization;
            });
        }

        void PoseGraph2D::FinishTrajectory(const int trajectory_id)
        {
            AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                CHECK(!IsTrajectoryFinished(trajectory_id));
                data_.trajectories_state[trajectory_id].state = TrajectoryState::FINISHED;

                for (const auto &submap : data_.submap_data.trajectory(trajectory_id))
                {
                    data_.submap_data.at(submap.id).state = SubmapState::kFinished;
                }
                return WorkItem::Result::kRunOptimization;
            });
        }

        bool PoseGraph2D::IsTrajectoryFinished(const int trajectory_id) const
        {
            return data_.trajectories_state.count(trajectory_id) != 0 &&
                   data_.trajectories_state.at(trajectory_id).state == TrajectoryState::FINISHED;
        }

        void PoseGraph2D::FreezeTrajectory(const int trajectory_id)
        {
            {
                absl::MutexLock locker(&mutex_);
                data_.trajectory_connectivity_state.Add(trajectory_id);
            }
            AddWorkItem([this, trajectory_id]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                CHECK(!IsTrajectoryFrozen(trajectory_id));
                // Connect multiple frozen trajectories among each other.
                // This is required for localization against multiple frozen trajectories
                // because we lose inter-trajectory constraints when freezing.
                for (const auto &entry : data_.trajectories_state)
                {
                    const int other_trajectory_id = entry.first;
                    if (!IsTrajectoryFrozen(other_trajectory_id))
                    {
                        continue;
                    }
                    if (data_.trajectory_connectivity_state.TransitivelyConnected(trajectory_id, other_trajectory_id))
                    {
                        // Already connected, nothing to do.
                        continue;
                    }
                    data_.trajectory_connectivity_state.Connect(trajectory_id, other_trajectory_id, common::FromUniversal(0));
                }
                data_.trajectories_state[trajectory_id].state = TrajectoryState::FROZEN;
                return WorkItem::Result::kDoNotRunOptimization;
            });
        }

        bool PoseGraph2D::IsTrajectoryFrozen(const int trajectory_id) const
        {
            return data_.trajectories_state.count(trajectory_id) != 0 &&
                   data_.trajectories_state.at(trajectory_id).state == TrajectoryState::FROZEN;
        }

        void PoseGraph2D::AddSubmapFromProto(const transform::Rigid3d &global_submap_pose, const proto::Submap &submap)
        {
            if (!submap.has_submap_2d())
            {
                return;
            }

            const SubmapId submap_id = {submap.submap_id().trajectory_id(), submap.submap_id().submap_index()};

            const transform::Rigid2d global_submap_pose_2d = transform::Project2D(global_submap_pose);
            {
                absl::MutexLock locker(&mutex_);
                const std::shared_ptr<const Submap2D> submap_ptr =
                    std::make_shared<const Submap2D>(submap.submap_2d(), &conversion_tables_);
                AddTrajectoryIfNeeded(submap_id.trajectory_id);
                if (!CanAddWorkItemModifying(submap_id.trajectory_id))
                    return;
                data_.submap_data.Insert(submap_id, InternalSubmapData());
                data_.submap_data.at(submap_id).submap = submap_ptr;
                // Immediately show the submap at the 'global_submap_pose'.
                data_.global_submap_poses_2d.Insert(submap_id, optimization::SubmapSpec2D{global_submap_pose_2d});
            }

            // TODO(MichaelGrupp): MapBuilder does freezing before deserializing submaps,
            // so this should be fine.
            if (IsTrajectoryFrozen(submap_id.trajectory_id))
            {
                kFrozenSubmapsMetric->Increment();
            }
            else
            {
                kActiveSubmapsMetric->Increment();
            }

            AddWorkItem([this, submap_id, global_submap_pose_2d]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                data_.submap_data.at(submap_id).state = SubmapState::kFinished;
                optimization_problem_->InsertSubmap(submap_id, global_submap_pose_2d);
                return WorkItem::Result::kDoNotRunOptimization;
            });
        }

        void PoseGraph2D::AddNodeFromProto(const transform::Rigid3d &global_pose, const proto::Node &node)
        {
            const NodeId node_id = {node.node_id().trajectory_id(), node.node_id().node_index()};
            std::shared_ptr<const TrajectoryNode::Data> constant_data =
                std::make_shared<const TrajectoryNode::Data>(FromProto(node.node_data()));

            {
                absl::MutexLock locker(&mutex_);
                AddTrajectoryIfNeeded(node_id.trajectory_id);
                if (!CanAddWorkItemModifying(node_id.trajectory_id))
                    return;
                data_.trajectory_nodes.Insert(node_id, TrajectoryNode{constant_data, global_pose});
            }

            AddWorkItem([this, node_id, global_pose]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                const auto &constant_data = data_.trajectory_nodes.at(node_id).constant_data;
                const auto gravity_alignment_inverse = transform::Rigid3d::Rotation(constant_data->gravity_alignment.inverse());
                optimization_problem_->InsertTrajectoryNode(
                    node_id, optimization::NodeSpec2D{
                                 constant_data->time, transform::Project2D(constant_data->local_pose * gravity_alignment_inverse),
                                 transform::Project2D(global_pose * gravity_alignment_inverse), constant_data->gravity_alignment});
                return WorkItem::Result::kDoNotRunOptimization;
            });
        }

        void PoseGraph2D::SetTrajectoryDataFromProto(const proto::TrajectoryData &data)
        {
            TrajectoryData trajectory_data;
            // gravity_constant and imu_calibration are omitted as its not used in 2d

            if (data.has_fixed_frame_origin_in_map())
            {
                trajectory_data.fixed_frame_origin_in_map = transform::ToRigid3(data.fixed_frame_origin_in_map());

                const int trajectory_id = data.trajectory_id();
                AddWorkItem([this, trajectory_id, trajectory_data]() LOCKS_EXCLUDED(mutex_) {
                    absl::MutexLock locker(&mutex_);
                    if (CanAddWorkItemModifying(trajectory_id))
                    {
                        optimization_problem_->SetTrajectoryData(trajectory_id, trajectory_data);
                    }
                    return WorkItem::Result::kDoNotRunOptimization;
                });
            }
        }

        void PoseGraph2D::AddNodeToSubmap(const NodeId &node_id, const SubmapId &submap_id)
        {
            AddWorkItem([this, node_id, submap_id]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                if (CanAddWorkItemModifying(submap_id.trajectory_id))
                {
                    data_.submap_data.at(submap_id).node_ids.insert(node_id);
                }
                return WorkItem::Result::kDoNotRunOptimization;
            });
        }

        void PoseGraph2D::AddSerializedConstraints(const std::vector<Constraint> &constraints)
        {
            AddWorkItem([this, constraints]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                for (const auto &constraint : constraints)
                {
                    CHECK(data_.trajectory_nodes.Contains(constraint.node_id));
                    CHECK(data_.submap_data.Contains(constraint.submap_id));
                    CHECK(data_.trajectory_nodes.at(constraint.node_id).constant_data != nullptr);
                    CHECK(data_.submap_data.at(constraint.submap_id).submap != nullptr);
                    switch (constraint.tag)
                    {
                    case Constraint::Tag::INTRA_SUBMAP:
                        CHECK(data_.submap_data.at(constraint.submap_id).node_ids.emplace(constraint.node_id).second);
                        break;
                    case Constraint::Tag::INTER_SUBMAP:
                        UpdateTrajectoryConnectivity(constraint);
                        break;
                    }
                    const Constraint::Pose pose = {
                        constraint.pose.zbar_ij *
                            transform::Rigid3d::Rotation(
                                data_.trajectory_nodes.at(constraint.node_id).constant_data->gravity_alignment.inverse()),
                        constraint.pose.translation_weight, constraint.pose.rotation_weight};
                    data_.constraints.push_back(Constraint{constraint.submap_id, constraint.node_id, pose, constraint.tag});
                }
                LOG(INFO) << "Loaded " << constraints.size() << " constraints.";
                return WorkItem::Result::kDoNotRunOptimization;
            });
        }

        void PoseGraph2D::AddTrimmer(std::unique_ptr<PoseGraphTrimmer> trimmer)
        {
            // C++11 does not allow us to move a unique_ptr into a lambda.
            PoseGraphTrimmer *const trimmer_ptr = trimmer.release();
            AddWorkItem([this, trimmer_ptr]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                trimmers_.emplace_back(trimmer_ptr);
                return WorkItem::Result::kDoNotRunOptimization;
            });
        }

        void PoseGraph2D::RunFinalOptimization()
        {
            {
                AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
                    absl::MutexLock locker(&mutex_);
                    optimization_problem_->SetMaxNumIterations(options_.max_num_final_iterations());
                    return WorkItem::Result::kRunOptimization;
                });
                AddWorkItem([this]() LOCKS_EXCLUDED(mutex_) {
                    absl::MutexLock locker(&mutex_);
                    optimization_problem_->SetMaxNumIterations(
                        options_.optimization_problem_options().ceres_solver_options().max_num_iterations());
                    return WorkItem::Result::kDoNotRunOptimization;
                });
            }
            WaitForAllComputations();
        }

        /// @brief 执行优化, 并使用优化结果对保存的数据进行更新
        void PoseGraph2D::RunOptimization()
        {
            auto nodeSize = optimization_problem_->node_data().size();
            auto submapSize = optimization_problem_->submap_data().size();
            LOG(INFO) << "[Pg2] RunOptimization constraintsSize " << data_.constraints.size();
            LOG(INFO) << "[Pg2] RunOptimization optimization: mapSize " << submapSize << " nodeSize " << nodeSize;
            LOG(INFO) << "[Pg2] RunOptimization data: mapSize " << data_.submap_data.size() << " nodeSize "
                      << data_.trajectory_nodes.size();
            if (submapSize < 1)
                return;
            submap_updator_.UpdateClosureRelations(data_.constraints);
            // No other thread is accessing the optimization_problem_,
            // data_.constraints, data_.frozen_trajectories and data_.landmark_nodes
            // when executing the Solve. Solve is time consuming, so not taking the mutex
            // before Solve to avoid blocking foreground processing.
            // Solve 比较耗时, 所以在执行 Solve 之前不要加互斥锁, 以免阻塞其他的任务处理; landmark直接参与优化问题
            // 这里是后端优化真正的入口
            // landmark数据与通过2个节点位姿插值出来的相对位姿的差值作为残差项
            optimization_problem_->Solve(data_.constraints, GetTrajectoryStates(), data_.landmark_nodes);
            absl::MutexLock locker(&mutex_);
            // 拷贝优化后的结果，注意优化问题和data_存在相同的数据，优化前从data_中拷贝，优化后更新到data_
            // submap_data的类型是 MapById<SubmapId, optimization::SubmapSpec2D>
            const auto &submap_data = optimization_problem_->submap_data();
            // node_data的类型是 MapById<NodeId, NodeSpec2D>
            // node_data是优化后的所有节点的新位姿
            const auto &node_data = optimization_problem_->node_data();
            LOG(INFO) << "OptEnd nodeSize " << node_data.size() << " mapSize " << submap_data.size();
            // 更新节点位姿
            int updatedNodeSize = 0;
            int updatedNodeSizeAft = 0;
            for (const int trajectory_id : node_data.trajectory_ids())
            {
                LOG(INFO) << "OptEnd trajid " << trajectory_id;
                for (const auto &node : node_data.trajectory(trajectory_id))
                {
                    updatedNodeSize++;
                    auto &mutable_trajectory_node = data_.trajectory_nodes.at(node.id);
                    // 对节点的全局位姿进行重力矫正恢复，因为优化过程处理的都是2d位姿
                    mutable_trajectory_node.global_pose =
                        transform::Embed3D(node.data.global_pose_2d) *
                        transform::Rigid3d::Rotation(mutable_trajectory_node.constant_data->gravity_alignment);
                }
                // Extrapolate all point cloud poses that were not included in the 'optimization_problem_' yet.
                // 优化之后该轨迹的位姿发生了变化，但优化过程与前端不是一个线程，所以在优化的过程中，该轨迹还有新的节点产生;
                // 用优化后的增量去修正后面新增的节点的全局位姿;
                const auto local_to_new_global = ComputeLocalToGlobalTransform(submap_data, trajectory_id);
                const auto local_to_old_global = ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, trajectory_id);
                const auto old_global_to_new_global = local_to_new_global * local_to_old_global.inverse();
                Print("OptEnd translation", old_global_to_new_global);
                // 从轨迹的最有一个节点往后修正，在定位模式时，不会去修改离线地图的位姿，因为离线地图的轨迹不会再新增节点，所以最后一个节点后面没有数据
                const NodeId last_optimized_node_id = std::prev(node_data.EndOfTrajectory(trajectory_id))->id;
                auto node_it = std::next(data_.trajectory_nodes.find(last_optimized_node_id));
                for (; node_it != data_.trajectory_nodes.EndOfTrajectory(trajectory_id); ++node_it)
                {
                    updatedNodeSizeAft++;
                    auto &mutable_trajectory_node = data_.trajectory_nodes.at(node_it->id);
                    mutable_trajectory_node.global_pose = old_global_to_new_global * mutable_trajectory_node.global_pose;
                    // LOG(INFO) << "OptEnd updateNode: trajId " << node_it->id.trajectory_id << " nodeidx " << node_it->id.node_index;
                }
            }
            LOG(INFO) << "OptEnd updatedNodeSize " << updatedNodeSize << " updatedNodeSizeAft " << updatedNodeSizeAft;
            // 更新路标数据
            for (const auto &landmark : optimization_problem_->landmark_data())
            {
                data_.landmark_nodes[landmark.first].global_landmark_pose = landmark.second;
            }
            // 从优化问题中拷贝子图的2d位姿数据
            data_.global_submap_poses_2d = submap_data;
            if (submap_data.Contains(SubmapId(1, 0)))
            {
                auto firstMapTrans = submap_data.at(SubmapId(1, 0)).global_pose.translation();
                LOG(INFO) << "[firstMapTrans] " << firstMapTrans(0) << " " << firstMapTrans(1);
            }
            // 更新子图
            AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) { return UpdateSubmap(); });
            LOG(INFO) << "RunOptimization finish ";
        }

        WorkItem::Result PoseGraph2D::UpdateSubmap()
        {
            absl::MutexLock locker(&mutex_);
            auto closures = submap_updator_.GetNeedUpdateSubmaps();
            LOG(INFO) << "UpdateSubmap " << closures.size();
            for (const auto &ci : closures)
            {
                Print("UpdateClosure", ci);
                const auto &node_id = ci.closure_id.node_id;
                const auto &submap_id = ci.closure_id.submap_id;
                const auto &nodeData = data_.trajectory_nodes.at(node_id);
                transform::Rigid3d localToGloabl = nodeData.global_pose * nodeData.constant_data->local_pose.inverse();
                const auto &rangeLocal2d = nodeData.constant_data->rangeLocal2d;
                sensor::RangeData rangeGlobal2d = sensor::TransformRangeData(rangeLocal2d, localToGloabl.cast<float>());
                const Submap2D *constSubmap = static_cast<const Submap2D *>(data_.submap_data.at(submap_id).submap.get());
                Submap2D *submap = const_cast<Submap2D *>(constSubmap);
                submap_updator_.InsertRange(submap, ci.closure_id, rangeGlobal2d);
            }
            return WorkItem::Result::kDoNotRunOptimization;
        }

        /// @brief 检查轨迹的状态，在FINISHED，DELETED，非NORMAL状态下不能给该轨迹加约束数据
        /// @param trajectory_id
        /// @return
        bool PoseGraph2D::CanAddWorkItemModifying(int trajectory_id)
        {
            auto it = data_.trajectories_state.find(trajectory_id);
            if (it == data_.trajectories_state.end())
            {
                return true;
            }
            if (it->second.state == TrajectoryState::FINISHED)
            {
                // TODO(gaschler): Replace all FATAL to WARNING after some testing.
                LOG(FATAL) << "trajectory_id " << trajectory_id
                           << " has finished "
                              "but modification is requested, skipping.";
                return false;
            }
            if (it->second.deletion_state != InternalTrajectoryState::DeletionState::NORMAL)
            {
                LOG(FATAL) << "trajectory_id " << trajectory_id
                           << " has been scheduled for deletion "
                              "but modification is requested, skipping.";
                return false;
            }
            if (it->second.state == TrajectoryState::DELETED)
            {
                LOG(FATAL) << "trajectory_id " << trajectory_id
                           << " has been deleted "
                              "but modification is requested, skipping.";
                return false;
            }
            return true;
        }

        MapById<NodeId, TrajectoryNode> PoseGraph2D::GetTrajectoryNodes() const
        {
            absl::MutexLock locker(&mutex_);
            return data_.trajectory_nodes;
        }

        MapById<NodeId, TrajectoryNodePose> PoseGraph2D::GetTrajectoryNodePoses() const
        {
            MapById<NodeId, TrajectoryNodePose> node_poses;
            absl::MutexLock locker(&mutex_);
            for (const auto &node_id_data : data_.trajectory_nodes)
            {
                absl::optional<TrajectoryNodePose::ConstantPoseData> constant_pose_data;
                if (node_id_data.data.constant_data != nullptr)
                {
                    constant_pose_data = TrajectoryNodePose::ConstantPoseData{node_id_data.data.constant_data->time,
                                                                              node_id_data.data.constant_data->local_pose};
                }
                node_poses.Insert(node_id_data.id, TrajectoryNodePose{node_id_data.data.global_pose, constant_pose_data});
            }
            return node_poses;
        }

        std::map<int, PoseGraphInterface::TrajectoryState> PoseGraph2D::GetTrajectoryStates() const
        {
            std::map<int, PoseGraphInterface::TrajectoryState> trajectories_state;
            absl::MutexLock locker(&mutex_);
            for (const auto &it : data_.trajectories_state)
            {
                trajectories_state[it.first] = it.second.state;
            }
            return trajectories_state;
        }

        std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses() const
        {
            std::map<std::string, transform::Rigid3d> landmark_poses;
            absl::MutexLock locker(&mutex_);
            for (const auto &landmark : data_.landmark_nodes)
            {
                // Landmark without value has not been optimized yet.
                if (!landmark.second.global_landmark_pose.has_value())
                    continue;
                landmark_poses[landmark.first] = landmark.second.global_landmark_pose.value();
            }
            return landmark_poses;
        }

        void PoseGraph2D::SetLandmarkPose(const std::string &landmark_id, const transform::Rigid3d &global_pose, const bool frozen)
        {
            AddWorkItem([=]() LOCKS_EXCLUDED(mutex_) {
                absl::MutexLock locker(&mutex_);
                data_.landmark_nodes[landmark_id].global_landmark_pose = global_pose;
                data_.landmark_nodes[landmark_id].frozen = frozen;
                return WorkItem::Result::kDoNotRunOptimization;
            });
        }

        sensor::MapByTime<sensor::ImuData> PoseGraph2D::GetImuData() const
        {
            absl::MutexLock locker(&mutex_);
            return optimization_problem_->imu_data();
        }

        sensor::MapByTime<sensor::OdometryData> PoseGraph2D::GetOdometryData() const
        {
            absl::MutexLock locker(&mutex_);
            return optimization_problem_->odometry_data();
        }

        std::map<std::string /* landmark ID */, PoseGraphInterface::LandmarkNode> PoseGraph2D::GetLandmarkNodes() const
        {
            absl::MutexLock locker(&mutex_);
            return data_.landmark_nodes;
        }

        std::map<int, PoseGraphInterface::TrajectoryData> PoseGraph2D::GetTrajectoryData() const
        {
            absl::MutexLock locker(&mutex_);
            return optimization_problem_->trajectory_data();
        }

        sensor::MapByTime<sensor::FixedFramePoseData> PoseGraph2D::GetFixedFramePoseData() const
        {
            absl::MutexLock locker(&mutex_);
            return optimization_problem_->fixed_frame_pose_data();
        }

        std::vector<PoseGraphInterface::Constraint> PoseGraph2D::constraints() const
        {
            std::vector<PoseGraphInterface::Constraint> result;
            absl::MutexLock locker(&mutex_);
            for (const Constraint &constraint : data_.constraints)
            {
                Constraint temp{constraint.submap_id, constraint.node_id,
                                Constraint::Pose{constraint.pose.zbar_ij *
                                                     transform::Rigid3d::Rotation(
                                                         data_.trajectory_nodes.at(constraint.node_id).constant_data->gravity_alignment),
                                                 constraint.pose.translation_weight, constraint.pose.rotation_weight},
                                constraint.tag};
                temp.func = constraint.func;
                temp.code = constraint.code;
                result.push_back(temp);
            }
            return result;
        }

        void PoseGraph2D::SetInitialTrajectoryPose(const int from_trajectory_id, const int to_trajectory_id, const transform::Rigid3d &pose,
                                                   const common::Time time)
        {
            absl::MutexLock locker(&mutex_);
            data_.initial_trajectory_poses[from_trajectory_id] = InitialTrajectoryPose{to_trajectory_id, pose, time};
        }

        transform::Rigid3d PoseGraph2D::GetInterpolatedGlobalTrajectoryPose(const int trajectory_id, const common::Time time) const
        {
            CHECK_GT(data_.trajectory_nodes.SizeOfTrajectoryOrZero(trajectory_id), 0);
            const auto it = data_.trajectory_nodes.lower_bound(trajectory_id, time);
            if (it == data_.trajectory_nodes.BeginOfTrajectory(trajectory_id))
            {
                return data_.trajectory_nodes.BeginOfTrajectory(trajectory_id)->data.global_pose;
            }
            if (it == data_.trajectory_nodes.EndOfTrajectory(trajectory_id))
            {
                return std::prev(data_.trajectory_nodes.EndOfTrajectory(trajectory_id))->data.global_pose;
            }
            return transform::Interpolate(transform::TimestampedTransform{std::prev(it)->data.time(), std::prev(it)->data.global_pose},
                                          transform::TimestampedTransform{it->data.time(), it->data.global_pose}, time)
                .transform;
        }

        // 局部系到全局系的变换，即累积的漂移量，即odom系到map系的变换
        transform::Rigid3d PoseGraph2D::GetLocalToGlobalTransform(const int trajectory_id) const
        {
            // 可能同时间有多个线程调用这同一个函数, 所以要加锁
            absl::MutexLock locker(&mutex_);
            return ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, trajectory_id);
        }

        std::vector<std::vector<int>> PoseGraph2D::GetConnectedTrajectories() const
        {
            absl::MutexLock locker(&mutex_);
            return data_.trajectory_connectivity_state.Components();
        }

        PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapData(const SubmapId &submap_id) const
        {
            absl::MutexLock locker(&mutex_);
            return GetSubmapDataUnderLock(submap_id);
        }

        MapById<SubmapId, PoseGraphInterface::SubmapData> PoseGraph2D::GetAllSubmapData() const
        {
            absl::MutexLock locker(&mutex_);
            return GetSubmapDataUnderLock();
        }

        MapById<SubmapId, PoseGraphInterface::SubmapPose> PoseGraph2D::GetAllSubmapPoses() const
        {
            absl::MutexLock locker(&mutex_);
            MapById<SubmapId, SubmapPose> submap_poses;
            for (const auto &submap_id_data : data_.submap_data)
            {
                auto submap_data = GetSubmapDataUnderLock(submap_id_data.id);
                submap_poses.Insert(submap_id_data.id, PoseGraph::SubmapPose{submap_data.submap->num_range_data(), submap_data.pose});
            }
            return submap_poses;
        }

        /// @brief 返回local系到全局系的转换关系;全局系一般是map系，如果配置文件要求提供odom系，那么local就是odom系，对于一个轨迹
        /// 来讲，局部坐标系就是轨迹开始时机器人的位姿，所以开始的时候odom与map重合，后面出现累计误差则odom出现漂移；odom的漂移量就是
        /// 该函数的返回值；具体的求法就是倒数第二个子图的优化后的位姿乘上它的局部位姿的逆，因为局部位姿是没有经过优化的；可以理解为odom的
        /// 漂移量就是前端匹配后的位姿和后端优化后的位姿之差；
        /// @param global_submap_poses 子图的全局位姿
        /// @param trajectory_id
        /// @return
        transform::Rigid3d PoseGraph2D::ComputeLocalToGlobalTransform(
            const MapById<SubmapId, optimization::SubmapSpec2D> &global_submap_poses, const int trajectory_id) const
        {
            auto begin_it = global_submap_poses.BeginOfTrajectory(trajectory_id);
            auto end_it = global_submap_poses.EndOfTrajectory(trajectory_id);
            if (begin_it == end_it)
            {
                // 在轨迹的第一个子图被优化前，返回值都是Identity，即local系与global系都是重合的;(?)第一个子图优化能否不要
                // 根据optimize_every_n_nodes等待
                const auto it = data_.initial_trajectory_poses.find(trajectory_id);
                if (it != data_.initial_trajectory_poses.end())
                {
                    // 该轨迹设置了初始的位姿
                    auto associate_trajectory_id = it->second.to_trajectory_id;
                    LOG(INFO) << "[initPose] " << trajectory_id << " associate " << associate_trajectory_id;
                    return GetInterpolatedGlobalTrajectoryPose(it->second.to_trajectory_id, it->second.time) * it->second.relative_pose;
                }
                else
                {
                    // 该轨迹没有设置初始位姿，所以初始的时候odom与map是重合的
                    return transform::Rigid3d::Identity();
                }
            }
            // 该轨迹最新已优化后的子图的ID
            const SubmapId last_optimized_submap_id = std::prev(end_it)->id;
            LOG(INFO) << "[LocalToGlobalPose] trajid " << trajectory_id << " submap " << last_optimized_submap_id.trajectory_id << " "
                      << last_optimized_submap_id.submap_index;
            // Accessing 'local_pose' in Submap is okay, since the member is const.
            // 最新已优化后的子图的全局位姿
            const auto &optimized_pose = transform::Embed3D(global_submap_poses.at(last_optimized_submap_id).global_pose);
            // 最新已优化后的子图的局部位姿(经过优化)
            const auto &local_pose = data_.submap_data.at(last_optimized_submap_id).submap->local_pose();
            // 最新已优化后的子图的全局位姿和局部位姿之差作为当前局部系到全局的相对变换量，即odom系到map系的漂移量
            // 理想情况下，无累积误差，odom系与map系重合，局部系与全局系重合
            return optimized_pose * local_pose.inverse();
        }

        PoseGraphInterface::SubmapData PoseGraph2D::GetSubmapDataUnderLock(const SubmapId &submap_id) const
        {
            const auto it = data_.submap_data.find(submap_id);
            if (it == data_.submap_data.end())
            {
                return {};
            }
            auto submap = it->data.submap;
            // 如果该子图已经优化过，则其位姿会在global_submap_poses_2d中保存
            if (data_.global_submap_poses_2d.Contains(submap_id))
            {
                // We already have an optimized pose.
                return {submap, transform::Embed3D(data_.global_submap_poses_2d.at(submap_id).global_pose)};
            }
            // 该子图没有优化，则找其所在轨迹中最后一个已优化的子图，用这个子图的全局位姿乘以该子图的相对位姿
            // We have to extrapolate.
            return {submap, ComputeLocalToGlobalTransform(data_.global_submap_poses_2d, submap_id.trajectory_id) * submap->local_pose()};
        }

        PoseGraph2D::TrimmingHandle::TrimmingHandle(PoseGraph2D *const parent)
            : parent_(parent)
        {
        }

        int PoseGraph2D::TrimmingHandle::num_submaps(const int trajectory_id) const
        {
            const auto &submap_data = parent_->optimization_problem_->submap_data();
            return submap_data.SizeOfTrajectoryOrZero(trajectory_id);
        }

        MapById<SubmapId, PoseGraphInterface::SubmapData> PoseGraph2D::TrimmingHandle::GetOptimizedSubmapData() const
        {
            MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
            for (const auto &submap_id_data : parent_->data_.submap_data)
            {
                if (submap_id_data.data.state != SubmapState::kFinished ||
                    !parent_->data_.global_submap_poses_2d.Contains(submap_id_data.id))
                {
                    continue;
                }
                submaps.Insert(submap_id_data.id,
                               SubmapData{submap_id_data.data.submap,
                                          transform::Embed3D(parent_->data_.global_submap_poses_2d.at(submap_id_data.id).global_pose)});
            }
            return submaps;
        }

        std::vector<SubmapId> PoseGraph2D::TrimmingHandle::GetSubmapIds(int trajectory_id) const
        {
            std::vector<SubmapId> submap_ids;
            const auto &submap_data = parent_->optimization_problem_->submap_data();
            for (const auto &it : submap_data.trajectory(trajectory_id))
            {
                submap_ids.push_back(it.id);
            }
            return submap_ids;
        }

        const MapById<NodeId, TrajectoryNode> &PoseGraph2D::TrimmingHandle::GetTrajectoryNodes() const
        {
            return parent_->data_.trajectory_nodes;
        }

        const std::vector<PoseGraphInterface::Constraint> &PoseGraph2D::TrimmingHandle::GetConstraints() const
        {
            return parent_->data_.constraints;
        }

        bool PoseGraph2D::TrimmingHandle::IsFinished(const int trajectory_id) const
        {
            return parent_->IsTrajectoryFinished(trajectory_id);
        }

        void PoseGraph2D::TrimmingHandle::SetTrajectoryState(int trajectory_id, TrajectoryState state)
        {
            parent_->data_.trajectories_state[trajectory_id].state = state;
        }

        void PoseGraph2D::TrimmingHandle::TrimSubmap(const SubmapId &submap_id)
        {
            // TODO(hrapp): We have to make sure that the trajectory has been finished
            // if we want to delete the last submaps.
            CHECK(parent_->data_.submap_data.at(submap_id).state == SubmapState::kFinished);

            // Compile all nodes that are still INTRA_SUBMAP constrained to other submaps
            // once the submap with 'submap_id' is gone.
            // We need to use node_ids instead of constraints here to be also compatible
            // with frozen trajectories that don't have intra-constraints.
            std::set<NodeId> nodes_to_retain;
            for (const auto &submap_data : parent_->data_.submap_data)
            {
                if (submap_data.id != submap_id)
                {
                    nodes_to_retain.insert(submap_data.data.node_ids.begin(), submap_data.data.node_ids.end());
                }
            }

            // Remove all nodes that are exlusively associated to 'submap_id'.
            std::set<NodeId> nodes_to_remove;
            std::set_difference(parent_->data_.submap_data.at(submap_id).node_ids.begin(),
                                parent_->data_.submap_data.at(submap_id).node_ids.end(), nodes_to_retain.begin(), nodes_to_retain.end(),
                                std::inserter(nodes_to_remove, nodes_to_remove.begin()));
            // 删除与子图有关的约束
            // Remove all 'data_.constraints' related to 'submap_id'.
            {
                std::vector<Constraint> constraints;
                for (const Constraint &constraint : parent_->data_.constraints)
                {
                    if (constraint.submap_id != submap_id)
                    {
                        constraints.push_back(constraint);
                    }
                }
                parent_->data_.constraints = std::move(constraints);
            }
            // 删除与子图的节点有关的约束
            // Remove all 'data_.constraints' related to 'nodes_to_remove'.
            // If the removal lets other submaps lose all their inter-submap constraints,
            // delete their corresponding constraint submap matchers to save memory.
            {
                std::vector<Constraint> constraints;
                // 记录被删掉的约束对应的子图ID
                std::set<SubmapId> other_submap_ids_losing_constraints;
                for (const Constraint &constraint : parent_->data_.constraints)
                {
                    if (nodes_to_remove.count(constraint.node_id) == 0)
                    {
                        constraints.push_back(constraint);
                    }
                    else
                    {
                        // A constraint to another submap will be removed, mark it as affected.
                        other_submap_ids_losing_constraints.insert(constraint.submap_id);
                    }
                }
                parent_->data_.constraints = std::move(constraints);
                // Go through the remaining constraints to ensure we only delete scan
                // matchers of other submaps that have no inter-submap constraints left.
                for (const Constraint &constraint : parent_->data_.constraints)
                {
                    if (constraint.tag == Constraint::Tag::INTRA_SUBMAP)
                    {
                        continue;
                    }
                    else if (other_submap_ids_losing_constraints.count(constraint.submap_id))
                    {
                        // 如果保留的约束对应的子图ID在other_submap_ids_losing_constraints中，表示这个子图还有用，需要保留
                        // 因为这个容器是记录失去所有约束inter约束的子图ID，这些子图将被删掉ScanMatcher
                        // This submap still has inter-submap constraints - ignore it.
                        other_submap_ids_losing_constraints.erase(constraint.submap_id);
                    }
                }
                // Delete scan matchers of the submaps that lost all constraints.
                // TODO(wohe): An improvement to this implementation would be to add the
                // caching logic at the constraint builder which could keep around only
                // recently used scan matchers.
                for (const SubmapId &submap_id : other_submap_ids_losing_constraints)
                {
                    parent_->constraint_builder_.DeleteScanMatcher(submap_id);
                }
            }

            // Mark the submap with 'submap_id' as trimmed and remove its data.
            CHECK(parent_->data_.submap_data.at(submap_id).state == SubmapState::kFinished);
            parent_->data_.submap_data.Trim(submap_id);
            parent_->constraint_builder_.DeleteScanMatcher(submap_id);
            parent_->optimization_problem_->TrimSubmap(submap_id);

            // We have one submap less, update the gauge metrics.
            kDeletedSubmapsMetric->Increment();
            if (parent_->IsTrajectoryFrozen(submap_id.trajectory_id))
            {
                kFrozenSubmapsMetric->Decrement();
            }
            else
            {
                kActiveSubmapsMetric->Decrement();
            }

            // Remove the 'nodes_to_remove' from the pose graph and the optimization
            // problem.
            for (const NodeId &node_id : nodes_to_remove)
            {
                parent_->data_.trajectory_nodes.Trim(node_id);
                parent_->optimization_problem_->TrimTrajectoryNode(node_id);
            }
        }

        MapById<SubmapId, PoseGraphInterface::SubmapData> PoseGraph2D::GetSubmapDataUnderLock() const
        {
            MapById<SubmapId, PoseGraphInterface::SubmapData> submaps;
            for (const auto &submap_id_data : data_.submap_data)
            {
                submaps.Insert(submap_id_data.id, GetSubmapDataUnderLock(submap_id_data.id));
            }
            return submaps;
        }

        void PoseGraph2D::SetGlobalSlamOptimizationCallback(PoseGraphInterface::GlobalSlamOptimizationCallback callback)
        {
            global_slam_optimization_callback_ = callback;
        }

        void PoseGraph2D::RegisterMetrics(metrics::FamilyFactory *family_factory)
        {
            auto *latency = family_factory->NewGaugeFamily("mapping_2d_pose_graph_work_queue_delay",
                                                           "Age of the oldest entry in the work queue in seconds");
            kWorkQueueDelayMetric = latency->Add({});
            auto *queue_size = family_factory->NewGaugeFamily("mapping_2d_pose_graph_work_queue_size", "Number of items in the work queue");
            kWorkQueueSizeMetric = queue_size->Add({});
            auto *constraints =
                family_factory->NewGaugeFamily("mapping_2d_pose_graph_constraints", "Current number of constraints in the pose graph");
            kConstraintsDifferentTrajectoryMetric = constraints->Add({{"tag", "inter_submap"}, {"trajectory", "different"}});
            kConstraintsSameTrajectoryMetric = constraints->Add({{"tag", "inter_submap"}, {"trajectory", "same"}});
            auto *submaps = family_factory->NewGaugeFamily("mapping_2d_pose_graph_submaps", "Number of submaps in the pose graph.");
            kActiveSubmapsMetric = submaps->Add({{"state", "active"}});
            kFrozenSubmapsMetric = submaps->Add({{"state", "frozen"}});
            kDeletedSubmapsMetric = submaps->Add({{"state", "deleted"}});
        }

    } // namespace mapping
} // namespace cartographer
